
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode.c
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "mode.h"

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool is_taking_off(mode_base_t mode);
static void bind_function(mode_base_t mode, mode_ops_t ops);
/*----------------------------------variable----------------------------------*/
static UserTakeOff takeoff;
static AutoYaw auto_yaw;

// altitude above-ekf-origin below which auto takeoff does not control horizontal position
static bool auto_takeoff_no_nav_active;
static float auto_takeoff_no_nav_alt_cm;

static struct mode_ops mode_base_ops = {
        .mode_number = NULL,
        .init        = NULL,
        .exit        = NULL,
        .run         = NULL,
        .requires_GPS = NULL,
        .has_manual_throttle = NULL,
        .allows_arming = NULL,
        .is_autopilot = NULL,
        .has_user_takeoff = NULL,
        .in_guided_mode = NULL,
        .logs_attitude = NULL,
        .allows_save_trim = NULL,
        .allows_autotune = NULL,
        .allows_flip = NULL,
        .name = NULL,
        .name4 = NULL,
        .is_taking_off = is_taking_off,
        .is_landing = NULL,
        .requires_terrain_failsafe = NULL,
        .get_wp = NULL,
        .wp_bearing = NULL,
        .wp_distance = NULL,
        .crosstrack_error = NULL,
        .output_to_motors = NULL,
        .use_pilot_yaw = NULL,
        .throttle_hover = NULL,
        .do_user_takeoff_start = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// 构造函数
void mode_ctor(mode_base_t mode, mode_ops_t ops)
{
    bind_function(mode, ops);

    mode->pos_control = fms.pos_control;
    mode->attitude_control = fms.attitude_control;
    mode->loiter_nav = fms.loiter_nav;
    mode->motors = fms.motors;
    mode->channel_roll = fms.channel_roll;
    mode->channel_pitch = fms.channel_pitch;
    mode->channel_throttle = fms.channel_throttle;
    mode->channel_yaw = fms.channel_yaw;

    mode->takeoff = &takeoff;
    mode->auto_yaw = &auto_yaw;
    mode->auto_takeoff_no_nav_active = &auto_takeoff_no_nav_active;
    mode->auto_takeoff_no_nav_alt_cm = &auto_takeoff_no_nav_alt_cm;
}

static void bind_function(mode_base_t mode, mode_ops_t ops)
{
    mode->ops = &mode_base_ops;

    if (ops->is_taking_off == NULL) {
        ops->is_taking_off = mode->ops->is_taking_off;
    }

    mode->ops = ops;
}

bool mode_is_disarmed_or_landed()
{
    if (!fms.motors->_armed || !fms.ap.auto_armed || fms.ap.land_complete) {
        return true;
    }
    return false;
}

void mode_zero_throttle_and_relax_ac(bool spool_up)
{
    if (spool_up) {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
    } else {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);
    }
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control, 0.0f, 0.0f, 0.0f);
    attctrl_set_throttle_out(fms.attitude_control,0.0f, false, fms.g.throttle_filt);
}

void mode_zero_throttle_and_hold_attitude()
{
    // run attitude controller
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control, 0.0f, 0.0f, 0.0f);
    attctrl_set_throttle_out(fms.attitude_control,0.0f, false, fms.g.throttle_filt);
}

AltHoldModeState mode_get_alt_hold_state(float target_climb_rate_cms)
{
    // Alt Hold State Machine Determination
    if (!fms.motors->_armed) {
        // the aircraft should moved to a shut down state
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_SHUT_DOWN);

        // transition through states as aircraft spools down
        switch (fms.motors->_spool_state) {

        case MOTOR_SHUT_DOWN:
            return AltHold_MotorStopped;

        case MOTOR_GROUND_IDLE:
            return AltHold_Landed_Ground_Idle;

        default:
            return AltHold_Landed_Pre_Takeoff;
        }

    } else if (takeoff_running(&takeoff) || takeoff_triggered(target_climb_rate_cms)) {
        // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
        // the aircraft should progress through the take off procedure
        return AltHold_Takeoff;

    } else if (!fms.ap.auto_armed || fms.ap.land_complete) {
        // the aircraft is armed and landed
        if (target_climb_rate_cms < 0.0f && !fms.ap.using_interlock) {
            // the aircraft should move to a ground idle state
            Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);

        } else {
            // the aircraft should prepare for imminent take off
            Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
        }

        if (fms.motors->_spool_state == MOTOR_GROUND_IDLE) {
            // the aircraft is waiting in ground idle
            return AltHold_Landed_Ground_Idle;

        } else {
            // the aircraft can leave the ground at any time
            return AltHold_Landed_Pre_Takeoff;
        }

    } else {
        // the aircraft is in a flying state
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
        return AltHold_Flying;
    }

}

void mode_set_throttle_takeoff()
{
    // initialise the vertical position controller
    posctrl_init_z_controller(fms.pos_control);
}

static bool is_taking_off(mode_base_t mode)
{
    if (!mode_has_user_takeoff(mode, false)) {
        return false;
    }
    if (fms.ap.land_complete) {
        return false;
    }
    return takeoff_running(mode->takeoff);
}

/*------------------------------------test------------------------------------*/


